﻿namespace TrueSync
{
    /// Rotation
    /// </summary>
    public struct Rot
    {
        /// Sine and cosine
        public FP s, c;
        public FP angle
        {
            get
            {
                return FP.Atan2(s, c);
            }
        }

        /// <summary>
        /// Initialize from an angle in radians
        /// </summary>
        /// <param name="angle">Angle in radians</param>
        public Rot(FP angle)
        {
            s = FP.FastSin(angle);
            c = FP.FastCos(angle);
        }

        /// <summary>
        /// Set using an angle in radians.
        /// </summary>
        /// <param name="angle"></param>
        public void Set(FP angle)
        {
            if (angle == 0)
            {
                s = 0;
                c = 1;
            }
            else
            {
                s = FP.FastSin(angle);
                c = FP.FastCos(angle);
            }
        }

        /// <summary>
        /// Set to the identity rotation
        /// </summary>
        public void SetIdentity()
        {
            s = FP.Zero;
            c = FP.One;
        }

        /// <summary>
        /// Get the x-axis
        /// </summary>
        public TSVector2 GetXAxis()
        {
            return new TSVector2(c, s);
        }

        /// <summary>
        /// Get the y-axis
        /// </summary>
        public TSVector2 GetYAxis()
        {
            return new TSVector2(-s, c);
        }

        public static TSVector2 Mul(Rot q, TSVector2 v)
        {
            return new TSVector2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
        }

        /// Inverse rotate a vector
        public static TSVector2 MulT(Rot q, TSVector2 v)
        {
            return new TSVector2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
        }

        public static Rot Mul(Rot q, Rot r)
        {
            Rot qr;
            qr.s = q.s * r.c + q.c * r.s;
            qr.c = q.c * r.c - q.s * r.s;
            return qr;
        }


        public static Rot MulT(Rot q, Rot r)
        {
            Rot qr;
            qr.s = q.c * r.s - q.s * r.c;
            qr.c = q.c * r.c + q.s * r.s;
            return qr;
        }
    }
}
